#include "RangeAndPwrMon.h"
#include "..\System\F2806x_Device.h"
#include "math.h"

bool RangeAndPwrMon::Init()
{
	// ADC INITIALISATION
	EALLOW;
	AdcRegs.ADCCTL1.bit.ADCBGPWD	= 1;	/* Power up band gap */
	DELAY_US(10000);						/* Delay before powering up rest of ADC */
	AdcRegs.ADCCTL1.bit.ADCREFSEL	= 0;	/* internal ref */
   	AdcRegs.ADCCTL1.bit.ADCREFPWD	= 1;	/* Power up reference */
   	AdcRegs.ADCCTL1.bit.ADCPWDN 	= 1;	/* Power up rest of ADC */
	AdcRegs.ADCCTL1.bit.ADCENABLE	= 1;	/* Enable ADC */
	DELAY_US(10000);
				
	AdcRegs.ADCCTL1.bit.INTPULSEPOS	= 1;	// create int pulses 1 cycle prior to output latch
							
	/******* CHANNEL SELECT *******/
	AdcRegs.ADCSOC0CTL.bit.CHSEL 	= 0;   	// ChSelect: ADC A0-> IR_NEAR
	AdcRegs.ADCINTSOCSEL1.bit.SOC0 	= 1;	// ADCInterrupt 1 causes SOC0
	AdcRegs.ADCSOC0CTL.bit.ACQPS 	= 6;	// Set SOC0 S/H Window to 7 ADC Clock Cycles, (6 ACQPS plus 1)

	AdcRegs.ADCSOC1CTL.bit.CHSEL 	= 1;   	// ChSelect: ADC A1-> IR_FAR
	AdcRegs.ADCINTSOCSEL1.bit.SOC1 	= 1;	// ADCInterrupt 1 causes SOC1
	AdcRegs.ADCSOC1CTL.bit.ACQPS 	= 6;	// Set SOC1 S/H Window to 7 ADC Clock Cycles, (6 ACQPS plus 1)

	AdcRegs.ADCSOC2CTL.bit.CHSEL 	= 2;   	// ChSelect: ADC A2-> SONIC
	AdcRegs.ADCINTSOCSEL1.bit.SOC2 	= 1;	// ADCInterrupt 1 causes SOC2
	AdcRegs.ADCSOC2CTL.bit.ACQPS 	= 6;	// Set SOC2 S/H Window to 7 ADC Clock Cycles, (6 ACQPS plus 1)

	AdcRegs.ADCSOC3CTL.bit.CHSEL 	= 5;   	// ChSelect: ADC A5-> Voltage
	AdcRegs.ADCINTSOCSEL1.bit.SOC3 	= 1;	// ADCInterrupt 1 causes SOC3
	AdcRegs.ADCSOC3CTL.bit.ACQPS 	= 6;	// Set SOC3 S/H Window to 7 ADC Clock Cycles, (6 ACQPS plus 1)

	AdcRegs.ADCSOC4CTL.bit.CHSEL 	= 6;   	// ChSelect: ADC A6-> Current
	AdcRegs.ADCINTSOCSEL1.bit.SOC4 	= 1;	// ADCInterrupt 1 causes SOC4
	AdcRegs.ADCSOC4CTL.bit.ACQPS 	= 6;	// Set SOC4 S/H Window to 7 ADC Clock Cycles, (6 ACQPS plus 1)

	
	// set continous mode
	AdcRegs.INTSEL1N2.bit.INT1SEL = 4;		// SOC4 (EOC4) causes ADCInterrupt 1
	AdcRegs.INTSEL1N2.bit.INT1CONT = 1;		// set ADCInterrupt 1 to auto clr
	AdcRegs.INTSEL1N2.bit.INT1E = 1;		// enable ADC interrupt 1					
								
	EDIS;
		
	AdcRegs.ADCSOCFRC1.all = 0x0010;  	// kick start ADC by causing a SOC4 event
		
	return true;
}

void RangeAndPwrMon::Update()
{
	// read regs
	float sonicVolt = AdcResult.ADCRESULT2*3.3f/4096.0f;
	m_Sonic = (sonicVolt+0.025f)/3.3f*512.0f*2.54f; // [cm]
	
	// IR Near
	float irNearVolt = AdcResult.ADCRESULT0*3.3f/4096.0f;
	m_IRNear =  356.7f*exp( -5.253f * irNearVolt) + 52.67f*exp(-0.7481f * irNearVolt);
	if( m_IRNear > 100 ) m_IRNear = 100; // clip to 100cm
	
	// IRfar
	float irFarVolt =  AdcResult.ADCRESULT1*3.3f/4096.0f;	
	m_IRFar = 317.9f*exp( -2.944f * irFarVolt) + 85.47f*exp(-0.5808f * irFarVolt);
	if(  m_IRFar > 200 ) m_IRFar = 200; // clip to 200cm
	
	// Voltage
	m_Voltage = AdcResult.ADCRESULT3*3.3f/4096.0f*110/10 * 1.070f;
	
	// Current
	float currVolt = AdcResult.ADCRESULT4*3.3f/4096.0f * 1.024f; // last term is correction! 
	m_Current = -((currVolt-2.50f)/0.625f)*50; // 50A range
}
